{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "7935e971",
   "metadata": {},
   "source": [
    "import"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "0645ea25",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "\n",
    "sys.path.append(\"./lib\")\n",
    "import rosbag\n",
    "\n",
    "from matplotlib.pyplot import cla\n",
    "from sensor_msgs import point_cloud2\n",
    "import cv2\n",
    "\n",
    "import os   \n",
    "import json\n",
    "import numpy as np\n",
    "from tqdm import tqdm\n",
    "import argparse\n",
    "import threading\n",
    "from cv_bridge import CvBridge, CvBridgeError\n",
    "\n",
    "bridge = CvBridge()\n",
    "\n",
    "\n",
    "from ddd_utils import draw_box_3d, project_to_image, compute_box_3d,rot_y2alpha,alpha2rot_y"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "010f29cc",
   "metadata": {},
   "source": [
    "内外参 HEADER\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "e95d857b",
   "metadata": {},
   "outputs": [],
   "source": [
    "HEADER = '''\\\n",
    "# .PCD v0.7 - Point Cloud Data file format\n",
    "VERSION 0.7\n",
    "FIELDS x y z intensity\n",
    "SIZE 4 4 4 4 \n",
    "TYPE F F F F \n",
    "COUNT 1 1 1 1 \n",
    "WIDTH {}\n",
    "HEIGHT 1\n",
    "VIEWPOINT 0 0 0 1 0 0 0\n",
    "POINTS {}\n",
    "DATA ascii\n",
    "'''\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "9c917903",
   "metadata": {},
   "outputs": [],
   "source": [
    "needed_choice = ['left_front', 'front_middle', 'right_front', 'left_back', 'right_back']\n",
    "\n",
    "\n",
    "Calibs= {\n",
    "\"left_front\":{\n",
    "    \"intrinsic\":\n",
    "        [ 9.8136791215576147e+02, 0., 9.4839461394621117e+02, 0.,0.,\n",
    "       9.7877930144468655e+02, 5.4826911056422762e+02,0., 0., 0., 1.,0.,],\n",
    "    \"extrinsic\":    \n",
    "        [-0.489303,-0.872083,-0.00725849,0.316294,\n",
    "      -0.0682373,0.0465809,-0.996582,-0.122966,\n",
    "      0.86944,-0.487136,-0.0823008,0.174502,\n",
    "      0,0,0,1]\n",
    "},\n",
    "\n",
    "\"front_middle\":{\n",
    "    \"intrinsic\":\n",
    "        [  9.6329358074361460e+02, 0., 1.0077748249221447e+03, 0.,0.,\n",
    "       9.6501237970612237e+02, 5.4370143226399443e+02, 0.,0., 0., 1.,0.,],\n",
    "    \"extrinsic\":    \n",
    "        [-0.00325009,0.999501,-0.0314101,0.0161996,\n",
    "      -0.972325,0.0041792,0.233596,-1.90735,\n",
    "      0.233611,0.0313,0.971827,-0.156316,\n",
    "      0,0,0,1]\n",
    "},\n",
    "\n",
    "\"right_front\":{\n",
    "    \"intrinsic\":\n",
    "        [9.9156983385556464e+02, 0., 9.5979848329359470e+02, 0., 0., \n",
    "       9.9052320215775444e+02, 5.3216083204490576e+02,  0., 0., 0., 1. , 0.],\n",
    "    \"extrinsic\":    \n",
    "        [0.38748,-0.920667,0.0472163,-0.516601,\n",
    "      -0.00512604,-0.0533686,-0.998562,-0.0777872,\n",
    "      0.921863,0.386681,-0.0253985,0.684171,\n",
    "      0,0,0,1]\n",
    "},\n",
    "\n",
    "\"left_back\":{\n",
    "    \"intrinsic\":\n",
    "        [ 9.7623060203681041e+02, 0., 9.4639844986342735e+02, 0.,0.,\n",
    "       9.7682136279895110e+02, 5.4500167321113429e+02,0., 0., 0., 1.,0.,],\n",
    "    \"extrinsic\":    \n",
    "        [0.991594,0.129371,-0.00216614,0.0266785,\n",
    "      -0.0018602,-0.00248605,-0.999995,0.339616,\n",
    "      -0.129376,0.991594,-0.00222448,-0.0266944,\n",
    "      0,0,0,1]\n",
    "},\n",
    "\n",
    "\"right_back\":{\n",
    "    \"intrinsic\":\n",
    "        [ 9.8506879793285611e+02, 0., 1.0038607766204225e+03, 0., 0.,\n",
    "       9.8454037729996605e+02, 5.4638714145710173e+02,0., 0., 0., 1.,0.,],\n",
    "    \"extrinsic\":    \n",
    "        [-0.977639,0.205631,0.0440235,0.292283,\n",
    "      -0.0489388,-0.0188763,-0.998624,-0.206619,\n",
    "      -0.204517,-0.978447,0.0285176,0.316011,\n",
    "      0,0,0,1]\n",
    "},\n",
    "\n",
    "\n",
    "\n",
    "}\n",
    "#  {'/iv_points':\"iv\",'/rslidar_points':\"m1\",'/os_cloud_node_1/points':\"ouster1\",'/os_cloud_node_2/points':\"ouster2\"}\n",
    "cam2lidar_map = {'left_front':'/os_cloud_node_1/points', 'front_middle':'/iv_points', \n",
    "                            'right_front':\"/os_cloud_node_2/points\", 'left_back':'/os_cloud_node_1/points',\n",
    "                            'right_back':\"/os_cloud_node_2/points\"}\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "081a76bc",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def loadlabel(label_path):\n",
    "    with open(label_path) as f:\n",
    "        labels = json.load(f)\n",
    "        objs = []\n",
    "        for label in labels:\n",
    "            obj = {}\n",
    "            position = label['psr']['position']\n",
    "            rotation = label['psr']['rotation']['z']\n",
    "            scale = label['psr']['scale']\n",
    "            x, y, z = position[\"x\"], position[\"y\"], position[\"z\"]\n",
    "            xs, ys, zs = scale[\"x\"], scale[\"y\"], scale[\"z\"]\n",
    "            obj[\"obj_type\"],obj[\"obj_id\"]  = label['obj_type'],label['obj_id']\n",
    "            obj[\"box\"] = [x, y, z, xs, ys, zs, rotation]\n",
    "            objs.append(obj)\n",
    "    return objs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "110a54e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "def write_pcd(points, save_pcd_path):\n",
    "    # points = []\n",
    "    # for p in gen:\n",
    "    #     points.append(p)\n",
    "    n = len(points)\n",
    "    lines = []\n",
    "    # print(\"n\",n)\n",
    "    for i in range(n):\n",
    "        # x, y, z, i, is_g, data_time = points[i]\n",
    "        x, y, z, i = points[i][:4]\n",
    "        lines.append('{:.6f} {:.6f} {:.6f} {}'.format( \\\n",
    "            x, y, z, i))\n",
    "    with open(save_pcd_path, 'w') as f:\n",
    "        f.write(HEADER.format(n, n))\n",
    "        f.write('\\n'.join(lines))\n",
    "\n",
    "\n",
    "def save_lidar(path,pair_dict):\n",
    "    points = []\n",
    "    for topic in arg.base_lidar:\n",
    "        lidar_dict = pair_dict[topic]\n",
    "        gen = lidar_dict[\"data\"]\n",
    "        for p in gen:\n",
    "            points.append(p)\n",
    "    write_pcd(points,path)\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e998bc74",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "96ae278f",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "27f5c23d",
   "metadata": {},
   "outputs": [],
   "source": [
    "def pair_to_B(A_list, B_list):\n",
    "    A_pair = []\n",
    "    B_pair = []\n",
    "    # 进行配对\n",
    "    for idx, A in enumerate(A_list):\n",
    "        time_gap_min = 100\n",
    "        A_value = A[0]\n",
    "        A_time = A[1]\n",
    "        for B in B_list:\n",
    "            if abs(A_time - B[1]) < time_gap_min:\n",
    "                time_gap_min = abs(A_time - B[1])\n",
    "                B_value = B[0]\n",
    "                B_time = B[1]\n",
    "\n",
    "        A_pair.append([A_value, A_time])\n",
    "        if time_gap_min > arg.time_threshold:\n",
    "            B_pair.append([None, None])\n",
    "        else:\n",
    "            B_pair.append([B_value, B_time])\n",
    "    return [A_pair, B_pair]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bac3f7a3",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "65ea2cb0",
   "metadata": {},
   "source": [
    "process_main"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "6efebe10",
   "metadata": {},
   "outputs": [],
   "source": [
    "# sava_path_map =  {'/iv_points':\"_990\",'/rslidar_points':\"_991\",' /os_cloud_node_1/points':\"_992\",'/ os_cloud_node_2/points':\"_993\"}\n",
    "\n",
    "def process_main(bag_path):\n",
    "    # 加载raw数据\n",
    "    print(\"=========process start!===========\")\n",
    "    res_bag_list = []\n",
    "    bag_file_list = os.listdir(bag_path)\n",
    "    labels = get_all_labels()\n",
    "    totalnum = len(bag_file_list)\n",
    "    t = tqdm(enumerate(bag_file_list))\n",
    "    for bag_idx, bag_file in t:\n",
    "        # if bag_idx <=10:\n",
    "        #     continue\n",
    "        # if not \"2022-09-05-16-08-58_7\" in bag_file:\n",
    "        #     continue\n",
    "        t.set_description(\"[{}/{}]\".format(bag_idx,totalnum))\n",
    "        bag_file_path = os.path.join(bag_path, bag_file)\n",
    "        if not os.path.isfile(bag_file_path):\n",
    "            continue\n",
    "        [bag_name,file_type] = bag_file.split(\"/\")[-1].split(\".\")\n",
    "        if not file_type==\"bag\":\n",
    "            continue\n",
    "        # global data_dict\n",
    "        data_dict = process_bag(bag_file_path)\n",
    "        # return \n",
    "        # compare_msg(data)\n",
    "        # if arg.thread:\n",
    "        #     threading.Thread(target=precess_,args=(bag_idx, bag_file)).start()\n",
    "        # else:\n",
    "        #     precess_(bag_idx, bag_file)\n",
    "        #     print(\"===================over===================\")\n",
    "        #     break\n",
    "        # break\n",
    "        process_cam(data_dict,labels)\n",
    "        \n",
    "    print(\"=========process over!===========\")\n",
    "\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "656af658",
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_label(boxs_3d):\n",
    "    # bboxs_3d.append([dim, location, rotation_y, box_2d[-1],class_name])\n",
    "    # col1 ： 类标签\n",
    "    # col5-8 ：2D bbox，左上和右下坐标\n",
    "    # col9-11 : 3D bbox dim 高、宽、长\n",
    "    # col12-14 ：3D bbox centor 坐标\n",
    "    # col15: 车体方向角\n",
    "    labels = []\n",
    "    for box_3d in boxs_3d:\n",
    "        # class_name = class_map[box_3d[4]]\n",
    "        class_name = box_3d[4]\n",
    "        obj_id= box_3d[5]\n",
    "        b3 = box_3d[3]\n",
    "        [bbox_xy0,bbox_xy1] =[max(b3[:, 0]),max(b3[:, 1])],[min(b3[:, 0]),min(b3[:, 1])]\n",
    "        dim = box_3d[0]\n",
    "        location = [_[0] for _ in box_3d[1].tolist()]\n",
    "        rotation = box_3d[2]\n",
    "        # label = [[class_name,class_id],0,0,0,bbox_xy0[0],bbox_xy0[1],bbox_xy1[0],bbox_xy1[1]]+dim+location+[rotation]\n",
    "        label = [class_name,obj_id,bbox_xy0[0],bbox_xy0[1],bbox_xy1[0],bbox_xy1[1]]+dim+location+[rotation]\n",
    "        labels.append(label)\n",
    "    return labels"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bb427bca",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "7217e8e8",
   "metadata": {},
   "source": [
    "showddd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "00b97280",
   "metadata": {},
   "outputs": [],
   "source": [
    "def showddd(img,calibs,objs,orientation):\n",
    "    imgs = None\n",
    "    bird_views = None\n",
    "    labels_list = []\n",
    "    bboxs_3d = []\n",
    "    (h,w,_) = img.shape\n",
    "    # img = cv2.imread(img_path)\n",
    "    bird_view = np.ones((out_size, out_size, 3), dtype=np.uint8) * 230\n",
    "    for obj in objs:\n",
    "        # 前 x 上 z 左 y\n",
    "        [x, y, z, xs, ys, zs, r] = obj[\"box\"]\n",
    "        if orientation == \"front_middle\":\n",
    "            x,z ,y= z,x,-y\n",
    "            xs, ys, zs = zs,-ys,xs\n",
    "\n",
    "        z -= zs/2\n",
    "        rotation_y = -r - np.pi/2\n",
    "        dim = [zs, ys, xs]\n",
    "        class_name = obj[\"obj_type\"]\n",
    "        if(class_name in arg.ignore_class):\n",
    "            continue\n",
    "        obj_id = obj[\"obj_id\"]\n",
    "        location = [x, y, z]\n",
    "        location = np.expand_dims(np.concatenate([location,np.ones(1)],0),1)\n",
    "        p = np.array(calibs[\"extrinsic\"]).reshape((4,4))[:3]\n",
    "        location = np.dot(p, location)\n",
    "        calib = np.array(calibs[\"intrinsic\"]).reshape((3,4))\n",
    "        box_3d = compute_box_3d(dim, location, rotation_y)\n",
    "        rect = box_3d[:4, [0, 2]]\n",
    "        for k in range(4):\n",
    "            rect[k] = project_3d_to_bird(rect[k])\n",
    "        lc = (250, 152, 12)\n",
    "        cv2.polylines(\n",
    "            bird_view, [rect.reshape(-1, 1, 2).astype(np.int32)],\n",
    "            True, lc, 2, lineType=cv2.LINE_AA)\n",
    "        # print(rect.mean(0))\n",
    "        r = rect.mean(0)\n",
    "        if r[0]<0 or r[1]<0 or r[0]>out_size or r[1]>out_size:\n",
    "            continue\n",
    "        cv2.putText(bird_view, str(obj_id)+str(class_name),(int(r[0]),int(r[1])) ,  \n",
    "                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0), 1)\n",
    "        # print(class_id,class_name,box_3d.mean(0),location.T)\n",
    "        if box_3d.mean() <0:\n",
    "            continue\n",
    "        # print(class_id,box_3d.mean(),box_3d.max(),box_3d.min())\n",
    "        box_2d = project_to_image(box_3d, calib) \n",
    "        # print(min(box_2d[:,0]),min(box_2d[:,1]),max(box_2d[:,0]),max(box_2d[:,1]))\n",
    "        if not (min(box_2d[:,0])<1920 or min(box_2d[:,1])<1080):\n",
    "            continue\n",
    "        if not (max(box_2d[:,0])>0 or max(box_2d[:,1])>0) :\n",
    "            continue\n",
    "\n",
    "\n",
    "        # print(box_2d)\n",
    "        # h = max(box_2d[:,1]) - min(box_2d[:,1])\n",
    "        # w = max(box_2d[:,0]) - min(box_2d[:,0])\n",
    "        # if abs(h) <10 or abs(w)<10  :\n",
    "        #     continue\n",
    "\n",
    "        # box_2d[box_2d>h]=h\n",
    "        # print(box_2d.mean(),class_id)\n",
    "\n",
    "        # if min(box_2d[-1])<-100 or box_2d[-1][0]>(1920+400) or box_2d[-1][1]>(1080+300) :\n",
    "        #     continue\n",
    "\n",
    "        # if box_2d.min()<-100  or box_2d.max()>10000  :\n",
    "        #     continue\n",
    "        # print(\"{} [{} {}] [{} {}]\".format(class_id,max(box_2d[:,0]),min(box_2d[:,0]),max(box_2d[:,1]),min(box_2d[:,1])))\n",
    "        img = draw_box_3d(img, box_2d)\n",
    "\n",
    "        bboxs_3d.append([dim, location, rotation_y, box_2d,class_name,obj_id])\n",
    "\n",
    "    labels = get_label(bboxs_3d)\n",
    "    if (len(labels)==0):\n",
    "        return None,None\n",
    "    for l in labels:\n",
    "        bbox =l[4:8]\n",
    "        xyxy = np.array(bbox)\n",
    "        class_id = l[0]\n",
    "        # print(class_id,xyxy)\n",
    "        # cv2.rectangle(img, (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3])), (255, 255, 0), 2)\n",
    "        cv2.rectangle(img, (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3])), (255, 255, 0), 2)\n",
    "        cv2.putText(img, str(class_id), (int(xyxy[2]), int(xyxy[3])),  \n",
    "                    cv2.FONT_HERSHEY_SIMPLEX, 2, (0,255,0), 2)\n",
    "\n",
    "    labels_list.append([labels])\n",
    "    \n",
    "\n",
    "    return labels_list,bird_view\n",
    "\n",
    "\n",
    "out_size = 400\n",
    "world_size = 80\n",
    "def project_3d_to_bird( pt):\n",
    "\n",
    "    pt[0] += world_size / 2\n",
    "    pt[1] = world_size - pt[1]\n",
    "    pt = pt * out_size / world_size\n",
    "    return pt.astype(np.int32)\n",
    "\n",
    "\n",
    "\n",
    "def save_label(labels_list,label_path):\n",
    "    for labels in labels_list:\n",
    "        with open(label_path,\"w\") as f:\n",
    "            for label in labels[0]:\n",
    "                s = \"\"\n",
    "                for l in label:\n",
    "                    s+= str(l)\n",
    "                    if not l is label[-1]:\n",
    "                        s += \" \"\n",
    "                if not label is labels[0][-1]:\n",
    "                    s += \"\\n\"\n",
    "                f.write(s)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "75d78879",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "f5b6b8f3",
   "metadata": {},
   "outputs": [],
   "source": [
    "def cam2lidar(img,orientation,pair_dict,cam_file_path,labels):\n",
    "    \n",
    "    imgc = img.copy()\n",
    "    lidarname = cam2lidar_map[orientation]\n",
    "    \n",
    "    bin_name = pair_dict[lidarname][\"bin_name\"]\n",
    "    # print(bin_name,orientation)\n",
    "    josn_name = bin_name.replace(\".bin\",\".json\")\n",
    "    if  josn_name not in labels:\n",
    "        return \n",
    "    label_path = os.path.join(labels[josn_name],josn_name)\n",
    "    objs = loadlabel(label_path)\n",
    "    imgc = img.copy()\n",
    "    labels_list,bird_view = showddd(imgc,Calibs[orientation],objs,orientation)\n",
    "    if(bird_view is None):\n",
    "        return \n",
    "    cv2.imwrite(cam_file_path, img)\n",
    "    cv2.imwrite(cam_file_path.replace(\"/images\",\"/images_3dbbox\"),imgc)\n",
    "    cv2.imwrite(cam_file_path.replace(\"/images\",\"/images_bev\").format(orientation),bird_view)\n",
    "\n",
    "    calib_file_path = cam_file_path.replace(\"/images\",\"/calibs\").replace(\".png\",\".json\")\n",
    "    with open(calib_file_path, 'w') as f:\n",
    "        json.dump(Calibs[orientation],f,indent=4, )\n",
    "\n",
    "    label_file_path = cam_file_path.replace(\"/images\",\"/labels\").replace(\".png\",\".txt\")\n",
    "    save_label(labels_list,label_file_path)\n",
    "    # import matplotlib.pyplot as plt\n",
    "    # from PIL import Image\n",
    "    # imgc = Image.fromarray(cv2.cvtColor(imgc, cv2.COLOR_BGR2RGB))  \n",
    "    # plt.imshow(imgc)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02f90f7b",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e03b9b18",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "42623387",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "903e0462",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "4ecb3edd",
   "metadata": {},
   "source": [
    "process_bag"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "c505310a",
   "metadata": {},
   "outputs": [],
   "source": [
    "def process_bag(bag_file):\n",
    "    print(\"=============>load {}\".format(bag_file.split(\"/\")[-1]))\n",
    "    bag = rosbag.Bag(bag_file)\n",
    "    info = bag.get_type_and_topic_info()\n",
    "    class_dict = info.topics\n",
    "    topics = arg.base_lidar+arg.camera_topic\n",
    "    bag_data = bag.read_messages(topics)\n",
    "    data_dict = {}\n",
    "    time_list = []\n",
    "    data_dict[\"bag_name\"] = bag_file.split(\"/\")[-1].split(\".\")[0]\n",
    "    for t in topics:\n",
    "        data_dict[t]=[]\n",
    "\n",
    "    for idx,(topic, msg, t) in enumerate(bag_data):\n",
    "        time = msg.header.stamp.secs + msg.header.stamp.nsecs / 1000000000.0\n",
    "        if topic in arg.base_lidar:\n",
    "            msg = point_cloud2.read_points(msg)\n",
    "        data_dict[topic].append({\"time\":time,\"data\":msg})\n",
    "        # time_list.append({\"time\":time,\"topic\":topic,\"data\":msg})\n",
    "    return data_dict\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "f389fde5",
   "metadata": {},
   "outputs": [],
   "source": [
    "topic2name =  {'/iv_points':\"iv\",'/rslidar_points':\"m1\",'/os_cloud_node_1/points':\"ouster1\",'/os_cloud_node_2/points':\"ouster2\"}\n",
    "# arg.ignore_class=[]\n",
    "def idx2binname(idx,bagname,topic):\n",
    "    idx_str = str(idx).zfill(4)\n",
    "    lidar_name = topic2name[topic]\n",
    "    bin_name = \"{}_frame_{}_{}.bin\".format(bagname,lidar_name,idx_str)\n",
    "    return bin_name\n",
    "\n",
    "def find_pair_lidar(cam_time,data_dict):\n",
    "    pair_dict = {}\n",
    "    for topic in arg.base_lidar:\n",
    "        lidar_dict = data_dict[topic]\n",
    "        time_gap_min = 10000\n",
    "        best_data = None\n",
    "        best_idx = -1\n",
    "        for idx,dd in enumerate(lidar_dict):\n",
    "            time_gap = abs(dd[\"time\"]-cam_time)\n",
    "            if (time_gap<1) and (time_gap<time_gap_min):\n",
    "                time_gap_min = time_gap\n",
    "                best_idx = idx+1\n",
    "                best_data = dd[\"data\"]\n",
    "        bin_name = idx2binname(best_idx,data_dict[\"bag_name\"],topic)\n",
    "        # pair_dict[topic]={\"bin_name\":bin_name,\"data\":best_data}\n",
    "        pair_dict[topic]={\"bin_name\":bin_name}\n",
    "    return pair_dict\n",
    "    \n",
    "def get_all_labels():\n",
    "    p1 = \"/data/percetion/boleidun/\"\n",
    "    labels = {}\n",
    "    for pi in os.listdir(p1):\n",
    "        p2 = os.path.join(p1,pi,\"label\")\n",
    "        if os.path.isdir(p2):\n",
    "            for pii in os.listdir(p2):\n",
    "                labels[pii]=p2\n",
    "    return labels\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "def process_cam(data_dict,labels):\n",
    "    pair_map = {}\n",
    "    cam_topic = arg.camera_topic[0]\n",
    "    orientation_choice = ['Top_view', 'left_front', 'front_middle', 'right_front', 'left_back', 'right_back', 'rearview',\n",
    "                        'Infrared']\n",
    "    needed_choice = ['left_front', 'front_middle', 'right_front', 'left_back', 'right_back']\n",
    "    for idx,dd in enumerate(data_dict[cam_topic]):\n",
    "        cam_time = dd[\"time\"]\n",
    "        cam_data = dd[\"data\"]\n",
    "        \n",
    "        pair_dict = find_pair_lidar(cam_time,data_dict)\n",
    "        # bin_path = \"./boleidun/lidar/{}_{}_{}.bin\".format(data_dict[\"bag_name\"],\"lidar\",str(idx).zfill(4)) \n",
    "        # save_lidar(bin_path,pair_dict)\n",
    "\n",
    "        for cam_i, image in enumerate(cam_data.raw_image):\n",
    "            orientation = orientation_choice[cam_i]\n",
    "            if (orientation not in needed_choice):\n",
    "                continue\n",
    "            cam_file_path = \"./boleidun/images/{}_{}_{}.png\".format(data_dict[\"bag_name\"],orientation,str(idx).zfill(4)) \n",
    "            img = bridge.imgmsg_to_cv2(image, desired_encoding='bgr8')\n",
    "            # if arg.distortion:\n",
    "            #     img = cv2.undistort(img, np.array(Breton_intrinsic[orientation]), np.array(aian_K[orientation]))\n",
    "            \n",
    "            cam2lidar(img,orientation,pair_dict,cam_file_path,labels)\n",
    "            # if(orientation==\"front_middle\"):\n",
    "                # break\n",
    "            # showddd(img)\n",
    "        # break\n",
    "\n",
    "        map_file_path = \"./boleidun/maps/{}_{}_{}.txt\".format(data_dict[\"bag_name\"],\"all\",str(idx).zfill(4)) \n",
    "        with open(map_file_path,\"w\") as f:\n",
    "            for pd in pair_dict:\n",
    "                bin_name = pair_dict[pd][\"bin_name\"]\n",
    "                f.write(bin_name+\"\\n\")\n",
    "        \n",
    "        # break\n",
    "\n",
    "\n",
    "        "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9a9f7410",
   "metadata": {},
   "source": [
    "主程序"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "9924e1bc",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "=========process start!===========\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "[0/5]: : 0it [00:00, ?it/s]"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "=============>load 2022-09-07-11-24-52_0.bag\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "[1/5]: : 1it [00:45, 45.90s/it]"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "=============>load 2022-09-07-11-27-29_3.bag\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "[2/5]: : 2it [01:32, 46.11s/it]"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "=============>load 2022-09-07-11-28-22_4.bag\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "[3/5]: : 3it [02:15, 44.63s/it]"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "=============>load 2022-09-07-11-30-06_6.bag\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "[4/5]: : 5it [02:58, 35.77s/it]"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "=========process over!===========\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\n"
     ]
    }
   ],
   "source": [
    "args = argparse.ArgumentParser()\n",
    "args.add_argument(\"--bags\", default=\"/data/percetion/博雷顿现场数据/0907/2/问题包\", type=str, dest='bags') # 0907/2/问题包\n",
    "args.add_argument(\"--target\", default=\"/data/percetion/fuyu/test\", type=str)\n",
    "args.add_argument(\"--force\", default=True)\n",
    "args.add_argument(\"--time_threshold\", type=float, default=0.05, dest='time_threshold')\n",
    "args.add_argument(\"--pair_topic\", nargs='+', type=str, default=['cam', 'points'], dest='pair_topic')\n",
    "args.add_argument(\"--base_lidar\", type=str,\n",
    "                  default=['/iv_points','/rslidar_points','/os_cloud_node_1/points','/os_cloud_node_2/points'],\n",
    "                  dest='base_lidar')\n",
    "args.add_argument(\"--camera_topic\", type=str,\n",
    "                  default=['/n_camera_gmsl_prep'])\n",
    "args.add_argument(\"--distortion\", type=bool, default=False, dest='distortion')\n",
    "args.add_argument(\"--save_lidar_type\", type=str, default='bin', dest='save_lidar_type')\n",
    "args.add_argument(\"--thread\", action='store_true', default=False,)\n",
    "args.add_argument(\"--ignore_class\", type=str,default=['Wall',\"Dust\"])\n",
    "arg = args.parse_args([])\n",
    "# arg.thread = True\n",
    "# dirs = [\"images_bev\",\"images_3dbbox\",\"images\",\"calibs\",\"labels\",\"maps\"]\n",
    "# for d in dirs:\n",
    "#     os.system(\"rm -r ./boleidun/{} ; mkdir ./boleidun/{}\".format(d,d))\n",
    "process_main(arg.bags)\n",
    "\n",
    "\n",
    "# 0：iv   /iv_points\n",
    "# 1：rslidar point /rslidar_points\n",
    "# 2: ouster left /os_cloud_node_1/points\n",
    "# 3: ouster right /os_cloud_node_2/points\n",
    "# camera  /n_camera_gmsl_prep"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "718c4dc8",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "77200043",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a64e998c",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "5cc31743",
   "metadata": {},
   "source": [
    "LAST"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "59b3c4c5",
   "metadata": {},
   "outputs": [],
   "source": [
    "# print(cam_file_path)\n",
    "# print(pair_dict)\n",
    "# print(len(data_dict[cam_topic]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "36d92a72",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "70fe91d8",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8df3105f",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "f1141509",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 0：iv   /iv_points\n",
    "# 1：rslidar /point rslidar_points\n",
    "# 2: ouster left /os_cloud_node_1/points\n",
    "# 3: ouster right /os_cloud_node_2/points\n",
    "# camera  /n_camera_gmsl_prep"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "718c4dc8",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "b439decf",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'2022-09-05-15-00-54_0_frame_iv_0007.bin'"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "\"2022-09-05-15-00-54_0_frame_iv_0007\"\n",
    "\"2022-09-05-15-00-54_0_frame_iv_0007.bin\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "17362c9c",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e0428342",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "af98435b",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "3d1d0ca6",
   "metadata": {},
   "outputs": [],
   "source": [
    "# import threading\n",
    "# from time import sleep\n",
    "# from random import random\n",
    "\n",
    "# num = 0\n",
    "\n",
    "# def add():\n",
    "#     global num\n",
    "#     for i in range(10):\n",
    "#         sleep(random())\n",
    "#         num = num + 1\n",
    "#         print(num,end=\" \")\n",
    "    \n",
    "# ts = []\n",
    "# for i in range(10):\n",
    "#     t = threading.Thread(target=add)\n",
    "#     t.start()\n",
    "#     ts.append(t)\n",
    "\n",
    "# for t in ts:\n",
    "#     t.join()\n",
    "# print(\"\\n 最后结果：\",num)\n"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "e1a1b001e220d928b91f8c0266777d646605be3640513aa809eb904b5d0cdf9a"
  },
  "kernelspec": {
   "display_name": "Python [conda env:tools]",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
